#include "fishbot_navigation_behaviors/set_initial_pose.hpp"



static const rclcpp::Logger LOGGER = rclcpp::get_logger("Behavior: SetInitialPose");

namespace fishbot::behaviors
{
    SetInitialPose::SetInitialPose(const std::string &name, const BT::NodeConfig &config,
                                   const std::shared_ptr<fishbot::behavior_tree::BehaviorContext> &shared_resources)
        : fishbot::behavior_tree::AsyncBehaviorBase(name, config, shared_resources)
    {
        RCLCPP_INFO_STREAM(LOGGER, "Init");
    }

    BT::PortsList SetInitialPose::providedPorts()
    {
        return BT::PortsList({
            BT::InputPort<std::string>("topic_name"),
            BT::InputPort<double>("covariance_x"),
            BT::InputPort<double>("covariance_y"),
            BT::InputPort<double>("covariance_theta"),
            BT::InputPort<geometry_msgs::msg::PoseStamped>("initial_pose"),
            BT::InputPort<double>("wait_time"),
            BT::OutputPort<bool>("success"),
        });
    }

    tl::expected<bool, std::string> SetInitialPose::doWork()
    {
        // 获取并检查输入
        geometry_msgs::msg::PoseStamped initial_pose;
        const auto maybe_initial_pose = getInput<geometry_msgs::msg::PoseStamped>("initial_pose");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_initial_pose))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "initial_pose maybe error");
            return tl::make_unexpected(std::string("No initial_pose found from"));
            ;
        }
        else
        {
            initial_pose = maybe_initial_pose.value();
        }

        std::string topic_name;
        const auto maybe_topic_name = getInput<std::string>("topic_name");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_topic_name))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "topic_name maybe error, use default value: /initialpose");
            topic_name = "/initialpose";
        }
        else
        {
            topic_name = maybe_topic_name.value();
        }

        double covariance_x;
        const auto maybe_covariance_x = getInput<double>("covariance_x");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_covariance_x))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "covariance_x maybe error, use default value: 0.25");
            covariance_x = 0.25;
        }
        else
        {
            covariance_x = maybe_covariance_x.value();
        }

        double covariance_y;
        const auto maybe_covariance_y = getInput<double>("covariance_y");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_covariance_y))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "covariance_y maybe error, use default value: 0.25");
            covariance_y = 0.25;
        }
        else
        {
            covariance_y = maybe_covariance_y.value();
        }

        double covariance_theta;
        const auto maybe_covariance_theta = getInput<double>("covariance_theta");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_covariance_theta))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "covariance_theta maybe error, use default value: 0.069");
            covariance_theta = 0.069;
        }
        else
        {
            covariance_theta = maybe_covariance_theta.value();
        }

        double wait_time;
        const auto maybe_wait_time = getInput<double>("wait_time");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_wait_time))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "covariance_x maybe error, use default value: 1.0");
            wait_time = 1.0;
        }
        else
        {
            wait_time = maybe_wait_time.value();
        }

        auto node = this->shared_resources_->node_;
        rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initial_pose;
        // 配置 QoS
        rclcpp::QoS qos_settings(5);
        qos_settings.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
        qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
        pub_initial_pose = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(topic_name, qos_settings);

        // 等待订阅者连接
        while (pub_initial_pose->get_subscription_count() == 0)
        {
            RCLCPP_INFO(node->get_logger(), "Waiting for subscribers...");
            rclcpp::Rate(2.0).sleep(); // 每0.5秒检查一次
        }

        geometry_msgs::msg::PoseWithCovarianceStamped pose_with_cov;
        pose_with_cov.header = initial_pose.header;
        pose_with_cov.pose.pose = initial_pose.pose;
        pose_with_cov.pose.covariance[0] = covariance_x;
        pose_with_cov.pose.covariance[7] = covariance_y;
        pose_with_cov.pose.covariance[35] = covariance_theta;

        rclcpp::Rate(1.0).sleep(); // 等待通道建立
        pub_initial_pose->publish(pose_with_cov);
        rclcpp::Rate(1.0 / wait_time).sleep(); // 等待消息发送

        setOutput<bool>("success", true);
        RCLCPP_INFO(rclcpp::get_logger("Behavior: SetInitialPose"), "Set initial pose");

        return true;
    }
}
